from wlkata_mirobot import WlkataMirobot
from modbus_tk.defines import READ_COILS,READ_INPUT_REGISTERS,READ_HOLDING_REGISTERS
import modbus_tk.defines as cst  
import time
import logging

robot_logger=logging.getLogger("Robot-Driver")

####示教点位

P_QR1 =  [-25.21,62.79,-21.44,-0.62,-41.6,-12,240.5,-113.43,55.47,-0.41,-0.34,-36.79,0]
P_QR2 =   [-11.33,53.47,-5.31,-0.8,-48.27,-26,239.35,-48.17,54.47,-0.43,-0.34,-36.73,0]
P_QR3 = [4.21,54.09,-4.75,-0.73,-49.31,-41.57,241.78,17.59,50.9,-0.38,-0.29,-36.8,0]
P_QR4 =  [18.84,59.66,-14.81,-0.97,-44.58,-55.91,243.04,82.62,52.29,-0.61,-0.42,-36.38,0]

P_Circle =  [10.56,15.27,58.92,-2.98,-74.1,-49.54,121.35,22.26,49.77,-0.61,-0.55,-36.12,0]
P_Triangle = [7.01,34.23,28.84,-1.75,-62.99,-44.63,183.43,22.21,50.42,-0.63,-0.49,-36.07,0]
P_Rectangle =  [25.56,38.75,19.41,-1.36,-57.78,-62.98,183.51,87.43,53.94,-0.67,-0.46,-36.26,0]
P_Sixangle =  [36.58,23.07,47.03,-1.86,-69.62,-74.61,119.11,88.05,49.99,-0.64,-0.48,-36.28,0]

P_TEMP = [18.77,36.73,-13.3,-0.72,-23.16,-55.45,243.57,82.5,140.43,-0.6,-0.39,-36.39,0]


b = WlkataMirobot()

class Robot_Driver():  
    b.home()
    Emer_flag =0

    def get_Robot_Status(self, ):
        try:    
            regValue = b.Master.get_status
            return regValue[0]
        except Exception as e:
            robot_logger.error("can not get robot {}'s status! Reason:{}".format(e))
            return None
        
    def get_KDI(self):
        pass

    def transfer(self, val):
        value_tmp = [val[0],val[1]]
        value_tmp.extend([42.31125, -1.295, 6.245, -27.343, 0,])
        self.x = value_tmp[0]
        self.y = value_tmp[1]
        self.z = value_tmp[2]
        self.a = value_tmp[3]
        self.b = value_tmp[4]
        self.c = value_tmp[5]
        self.d = value_tmp[6]
        
        return self.x,self.y,self.z,self.a,self.b,self.c,self.d

    def robot_Action(self, value, cmd):
        if value != []:
            self.transfer(value)
            z_tmp = self.z + 50 
            
            match cmd:
                case 19:
                    b.home()
                    time.sleep(1)
                case 20:
                    b.home()
                    time.sleep(1)
                #### Harmful wastes
                case 11:
                    ### 拍照区的检测、抓取
                    self.x = self.x + 8
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    time.sleep(1)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置到二维码位置，显示器正面，最左端
                    b.set_joint_angle(P = P_QR1)
                    b.Master.set_DO(0,0)
                    time.sleep(1)
                #### Recycle wastes
                case 12:
                    ### 拍照区的检测、抓取
                    self.x = self.x + 8
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置操作
                    b.set_joint_angle(P = P_QR2)
                    
                    b.Master.set_DO(0,0)
                #### Kitchen wastes
                case 13:
                    ### 拍照区的检测、抓取
                    self.x = self.x + 8
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)

                    ### 放置操作
                    
                    b.set_joint_angle(P = P_QR3)
                    b.Master.set_DO(0,0)
                #### Other wastes
                case 14:
                    ### 拍照区的检测、抓取
                    self.x = self.x + 8
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置到二维码位置，显示器正面，最右端
                    b.set_joint_angle(P = P_TEMP)
                    b.set_joint_angle(P = P_QR4)
                    b.Master.set_DO(0,0)

                #### QR1
                case 21:
                    ### 拍照区的检测、抓取
                    self.x = self.x +10
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    time.sleep(1)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    time.sleep(1)
                    ### 放置到二维码位置，显示器正面，最左端
                    b.set_joint_angle(P = P_QR1)
                    b.Master.set_DO(0,0)
                    time.sleep(1)
                #### QR2
                case 22:
                    ### 拍照区的检测、抓取
                    self.x = self.x +10
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置操作
                    b.set_joint_angle(P = P_QR2)
                    b.Master.set_DO(0,0)
                #### QR3
                case 23:
                    ### 拍照区的检测、抓取
                    self.x = self.x +10 
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置操作
                    b.set_joint_angle(P = P_QR3)
                    b.Master.set_DO(0,0)
                #### QR4
                case 24:
                    ### 拍照区的检测、抓取
                    self.x = self.x +10
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置到二维码位置，显示器正面，最右端
                    b.set_joint_angle(P= P_TEMP)
                    b.set_joint_angle(P= P_QR4)
                    b.Master.set_DO(0,0)

                #### Circle
                case 31:
                    ### 拍照区的检测、抓取
                    self.x = self.x + 10
                    ### 抓取点上方
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)                    
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置到二维码位置，显示器正面，最左端
                    b.set_joint_angle(P = P_Circle)
                    b.Master.set_DO(0,0)
                #### Triangle
                case 32:
                    ### 拍照区的检测、抓取
                    ### 抓取点上方
                    self.x = self.x + 6
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置操作
                    b.set_joint_angle(P =P_Triangle)
                    b.Master.set_DO(0,0)
                #### Rectangle
                case 33:
                    ### 拍照区的检测、抓取
                    ### 抓取点上方
                    self.x = self.x + 10
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置操作
                    b.set_joint_angle(P = P_Rectangle)
                    b.Master.set_DO(0,0)
                #### hexagon
                case 34:
                    ### 拍照区的检测、抓取
                    ### 抓取点上方
                    self.x = self.x + 10
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 抓取点
                    b.set_tool_pose(x = self.y, y = self.x, z = self.z, roll= self.a, pitch = self.b, yaw= self.c)
                    #### 吸气
                    b.Master.set_DO(0,1)
                    time.sleep(1)
                    b.set_tool_pose(x = self.y, y = self.x, z = z_tmp, roll= self.a, pitch = self.b, yaw= self.c)
                    ### 放置到二维码位置，显示器正面，最右端
                    b.set_joint_angle(P = P_Sixangle)
                    b.Master.set_DO(0,0)

        else:
            b.home()    
 